public class GpsToCityPlanar {

    // 仿射变换参数：用于 x = a * lon + b * lat + c
    //               y = d * lon + e * lat + f
    private static double a, b, c, d, e, f;

    public static void fit(double[] lons, double[] lats, double[] xs, double[] ys) {
        if (lons.length != lats.length || xs.length != ys.length || lons.length != xs.length)
            throw new IllegalArgumentException("输入数组长度必须一致");

        int n = lons.length;

        // 构造线性方程组求解系数 a,b,c,d,e,f
        double Sll = sum(lons, lons);       // Σ(lon^2)
        double Slt = sum(lons, lats);       // Σ(lon*lat)
        double Sl = sum(lons);              // Σ(lon)
        double Stt = sum(lats, lats);       // Σ(lat^2)
        double St = sum(lats);              // Σ(lat)

        double Sxl = sumMultiply(xs, lons);
        double Sxt = sumMultiply(xs, lats);
        double Sx = sum(xs);

        double Syl = sumMultiply(ys, lons);
        double Syt = sumMultiply(ys, lats);
        double Sy = sum(ys);

        // 求解 [a, b, c] for x = a*lon + b*lat + c
        solve3x3(Sll, Slt, Sl,
                Slt, Stt, St,
                Sl,  St,  n, Sxl, Sxt, Sx);

        a = result[0];
        b = result[1];
        c = result[2];

        // 求解 [d, e, f] for y = d*lon + e*lat + f
        solve3x3(Sll, Slt, Sl,
                Slt, Stt, St,
                Sl,  St,  n, Syl, Syt, Sy);

        d = result[0];
        e = result[1];
        f = result[2];
    }

    private static double[] result = new double[3];

    // 解三元一次方程组 Ax = B
    private static void solve3x3(double a11, double a12, double a13,
                                 double a21, double a22, double a23,
                                 double a31, double a32, double a33,
                                 double b1, double b2, double b3) {
        double det = determinant3x3(a11, a12, a13,
                a21, a22, a23,
                a31, a32, a33);

        if (Math.abs(det) < 1e-9) throw new RuntimeException("矩阵奇异，无法求解");

        result[0] = determinant3x3(b1, a12, a13,
                b2, a22, a23,
                b3, a32, a33) / det;

        result[1] = determinant3x3(a11, b1, a13,
                a21, b2, a23,
                a31, b3, a33) / det;

        result[2] = determinant3x3(a11, a12, b1,
                a21, a22, b2,
                a31, a32, b3) / det;
    }

    private static double determinant3x3(double a11, double a12, double a13,
                                         double a21, double a22, double a23,
                                         double a31, double a32, double a33) {
        return a11 * (a22 * a33 - a23 * a32)
                - a12 * (a21 * a33 - a23 * a31)
                + a13 * (a21 * a32 - a22 * a31);
    }

    private static double sum(double[] arr) {
        double s = 0;
        for (double v : arr) s += v;
        return s;
    }

    private static double sum(double[] a, double[] b) {
        double s = 0;
        for (int i = 0; i < a.length; i++) s += a[i] * b[i];
        return s;
    }

    private static double sumMultiply(double[] a, double[] b) {
        double s = 0;
        for (int i = 0; i < a.length; i++) s += a[i] * b[i];
        return s;
    }

    public static double[] transform(double lon, double lat) {
        double x = a * lon + b * lat + c;
        double y = d * lon + e * lat + f;
        return new double[]{x, y};
    }

    public static void main(String[] args) {
        // 训练样本
        double[] lons = {121.495191023, 121.01491172, 121.925874523, 121.345370351};
        double[] lats = {31.2419292248, 31.1005887527, 30.9092357629, 31.4036845977};

        double[] xs = {2671.5952072, -43144.1660767, 43852.7672729, -11581.2196045};
        double[] ys = {718.49670516, -14865.9168091, -36076.2282104, 18658.682312};

        fit(lons, lats, xs, ys);

        System.out.println("Affine model fitted:");
        System.out.printf("x = %.6f * lon + %.6f * lat + %.6f\n", a, b, c);
        System.out.printf("y = %.6f * lon + %.6f * lat + %.6f\n", d, e, f);

        // 测试转换
        double[] testPoint = transform(121.495191023, 31.2419292248);
        System.out.printf("Predicted: (%.6f, %.6f)\n", testPoint[0], testPoint[1]);
    }
}